#include "app.h"

#include "input.h"
#include <cstdio>
#include "shapes3d.h"
#include "fonts\bitmap_font.h"
#include "random.h"

#include "collision_primitive.h"
#include "collisiondetector.h"
#include "physics_object.h"


#include <utils.hpp>
using namespace siut::simd;

#include <Vec2f.hpp>
using siut::simd::Vec2f;


void _spheresVsBoxes( std::vector<PhysicsObject*> &physObjList )
{
	

	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	float x = 0;
	float y = 0;
	float z = 0;
	float dist = 1.2f;
	// Arrange in a cube, get cube root of number of balls to decide num to make on a side
	//int limit = (int) real_pow( int(MAX_BALLS-1), 1.0f/3.0f )-1;
	int limit = 2;


	for(int i=0; i<MAX_BALLS; i++)
	{
		Vec3f halfSize = Vec3f(0.5f, 0.5f, 0.5f);
		real mass = (real) 1.0;
		Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
		Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

		PhysicsObject *ball = new Ball;
		physObjList.push_back(ball);
		ball->vSetState( -limit*0.5f*dist + x * dist, 5.0f + y*dist, -limit*0.5f*dist +  z * dist, orientation, halfSize, mass, acceleration);
		ball->collisionPrimitive->body->setPosition(ball->collisionPrimitive->body->getPosition() + Vec3f(5.0f) );
		ball->collisionPrimitive->body->calculateDerivedData();
		ball->collisionPrimitive->calculateInternals();

		x++;
		if( x > limit)
		{
			x = 0;
			z++;
		}
		if( z > limit)
		{
			z = 0;
			y++;
		}
	}


	for (int i=0; i<MAX_BOXES; i++)
	{		
		Vec3f position = Vec3f(0.0f, 1.5f, 0.0f);
		Vec3f halfSize = Vec3f(0.5f, 0.5f, 0.5f);
		real mass = (real) 5.0;
		Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
		Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

		if( i == MAX_BOXES-1)
		{
			position.is(5,6,5);
			halfSize = Vec3f(4.0, 0.5, 4.0);
			// High mass to make it near immovable my other impacts
			mass = 1e10f;
			acceleration = Vec3f(0.0f, 0.0f, 0.0f );
			orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
		}
		else if( i == MAX_BOXES-2)
		{
			position.is(5,3,-6);
			halfSize = Vec3f(2.5, 0.5, 6.0);

			// High mass to make it near immovable my other impacts
			mass = 1e10f;
			acceleration = Vec3f(0.0f, 0.0f, 0.0f );
			orientation = Quatf(0.25f, 1.0f, 0.0f, 0.0f) ;

		}else
		{
			float x = -(MAX_BOXES / 2)*1.2f + i * 1.2f;
			float y = 3.5f;
			float z = -15;
			position.is(x,y,z);
			halfSize = Vec3f(0.5, 0.5, 0.5);
			acceleration = Vec3f(0.0f, -9.81f, 0);
			orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
			mass = 1.0f;
		}

		PhysicsObject *box = new Box;
		box->vSetState( position.x(), position.y(), position.z(), orientation, halfSize, mass, acceleration);
		physObjList.push_back(box);
	}

	PhysicsObject *bigBox = new Box;

	Vec3f halfSize = Vec3f(25.0f, 1.0f, 25.0f);
	real mass = (real) 1e16;
	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);
}

void _dominos( std::vector<TorPhysix::PhysicsObject*> &physObjList )
{
	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	int numObjs = 25;
	for( int i=0; i<numObjs; i++)
	{
		real ang =  (real) i/(real)numObjs * 2.0f * 3.14159f;
		real radi = (real ) 14.0f;

		real dist_tween = (real)2.5;
		real x = cos(ang) * radi;
		real y = (real) 2.01;
		real z = sin(ang) * radi;
		Quatf orient;
		FromAxis(orient, Vec3f(0.0f, 1.0f, 0.0f), -ang );
		Vec3f siz(1.0f, 2.0f, 0.25f);
		Vec3f accel(0.0f, -20.0f, 0.0f);
		real mass = (real) 1.0;

		if( i == numObjs-1)
		{
			//y = (real) 4.0;
			//orient = Quatf( 25.0f/180.0f*3.14f , 1.0f, 0.0f, 0.0f);

			TorPhysix::PhysicsObject* domino = new Box;
			domino->vSetState( x,y,z, orient, siz, mass, accel);
			physObjList.push_back(domino);

			domino->collisionPrimitive->body->setVelocity( Vec3f(3.0f, 1.0f, 3.0f) );
		}else
		{

			TorPhysix::PhysicsObject* domino = new Box;
			domino->vSetState( x,y,z, orient, siz, mass, accel);
			physObjList.push_back(domino);
		}



	}


	PhysicsObject *bigBox = new Box;

	Vec3f halfSize = Vec3f(30.0f, 1.0f, 30.0f);
	real mass = (real) 1e10;
	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);

}

void _cubeStack( std::vector<TorPhysix::PhysicsObject*> &physObjList )
{
	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	real spacing = (real) 0.01;
	int x = 0;
	int y = 1;
	int z = 0;
	real limit = 2;

	for(int i=0; i<3*3*3; i++)
	{
		Quatf orient(0.0f, 1.0f, 0.0f, 0.0f);
		Vec3f siz(0.5f, 0.5f, 0.5f);
		Vec3f accel(0.0f, -15.0f, 0.0f);
		real mass = (real) 1.0;


		TorPhysix::PhysicsObject* domino = new Box;
		domino->vSetState( x + x * spacing, y + y * spacing ,z + z * spacing, orient, siz, mass, accel);
		physObjList.push_back(domino);

		x++;
		if( x > limit)
		{
			x = 0;
			z++;
		}
		if( z > limit)
		{
			z = 0;
			y++;
		}
	}

	PhysicsObject *bigBox = new Box;

	Vec3f halfSize = Vec3f(10.0f, 1.0f, 30.0f);
	real mass = (real) 1e16;
	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);
}

void _funnel( std::vector<TorPhysix::PhysicsObject*> &physObjList )
{
	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	real spacing = (real) 0.6;
	int x = 0;
	int y = 15;
	int z = 0;
	real limit = 2;

	for(int i=0; i<3*3*3; i++)
	{
		Quatf orient(0.0f, 1.0f, 0.0f, 0.0f);
		Vec3f siz(0.5f, 0.5f, 0.5f);
		Vec3f accel(0.0f, -15.0f, 0.0f);
		real mass = (real) 1.0;


		TorPhysix::PhysicsObject* domino = new Box;
		domino->vSetState( (real) -1.5 + x + x * spacing, (real) y + y * spacing , (real) -1.5 + z + z * spacing, orient, siz, mass, accel);
		physObjList.push_back(domino);

		x++;
		if( x > limit)
		{
			x = 0;
			z++;
		}
		if( z > limit)
		{
			z = 0;
			y++;
		}
	}



	// Funnel walls
	Vec3f halfSize = Vec3f(5.0f, 0.25f, 5.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );
	real mass = (real) 1e16;
	real yPos = 10;
	real tiltAngle = 40.0f / 180.0f * 3.14159f;

	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	FromAxis(orientation, Vec3f(1.0f, 0.0f, 0.0f), tiltAngle );
	PhysicsObject *wall1 = new Box;
	wall1->vSetState( 0.0f, yPos, -4.6f, orientation, halfSize, mass, acceleration);
	wall1->collisionPrimitive->body->setStatic( true );
	physObjList.push_back(wall1);

	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	FromAxis(orientation, Vec3f(1.0f, 0.0f, 0.0f), -tiltAngle );
	PhysicsObject *wall2 = new Box;
	wall2->vSetState( 0.0f, yPos, 4.6f, orientation, halfSize, mass, acceleration);
	wall2->collisionPrimitive->body->setStatic( true );
	physObjList.push_back(wall2);

	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	FromAxis(orientation, Vec3f(0.0f, 0.0f, 1.0f), tiltAngle );
	PhysicsObject *wall3 = new Box;
	wall3->vSetState( 4.6f, yPos, 0.0f, orientation, halfSize, mass, acceleration);
	wall3->collisionPrimitive->body->setStatic( true );
	physObjList.push_back(wall3);

	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	FromAxis(orientation, Vec3f(0.0f, 0.0f, 1.0f), -tiltAngle );
	PhysicsObject *wall4 = new Box;
	wall4->vSetState( -4.6f, yPos, 0.0f, orientation, halfSize, mass, acceleration);
	wall4->collisionPrimitive->body->setStatic( true );
	physObjList.push_back(wall4);

	PhysicsObject *bigBox = new Box;

	halfSize = Vec3f(10.0f, 1.0f, 30.0f);
	mass = (real) 1e16;
	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	acceleration = Vec3f(0.0f, 0.0f, 0.0f );

	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);
}


void _jengaCubeStack( std::vector<TorPhysix::PhysicsObject*> &physObjList )
{
	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	real spacing = (real) 0.001;

	int y = 0;
	real startY = (real) 0.5 + spacing;

	for(int i=0; i<4; i++)
	{
		Quatf orient(0.0f, 1.0f, 0.0f, 0.0f);
		Vec3f siz1(0.5f, 0.5f, 0.5f);
		Vec3f siz2(0.5f, 0.5f, 0.5f);
		Vec3f accel(0.0f, -15.0f, 0.0f);
		real mass = (real) 1.0;

		real x1, x2 = 0;
		real z1, z2 = 0;

		if( i % 2 == 0)
		{
			x1 = -2;
			x2 = 2;
			z1 = 0;
			z2 = 0;
			siz1 = Vec3f(0.5f, 0.5f, 3.0f);
			siz2 = Vec3f(0.5f, 0.5f, 3.0f);

		}else
		{
			x1 = 0;
			x2 = 0;
			z1 = -2;
			z2 = 2;

			siz1 = Vec3f(3.0f, 0.5f, 0.5f);
			siz2 = Vec3f(3.0f, 0.5f, 0.5f);
		}
		TorPhysix::PhysicsObject* one = new Box;
		one->vSetState( x1, startY + y + y * spacing ,z1, orient, siz1, mass, accel);

		TorPhysix::PhysicsObject* two = new Box;
		two->vSetState( x2, startY + y + y * spacing ,z2, orient, siz2, mass, accel);

		physObjList.push_back(one);
		physObjList.push_back(two);

		y += 1;

	}

	PhysicsObject *bigBox = new Box;

	Vec3f halfSize = Vec3f(10.0f, 1.0f, 30.0f);
	real mass = (real) 1e16;
	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);
}

void _inclinedPlane( std::vector<TorPhysix::PhysicsObject*> &physObjList )
{
	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	Vec3f halfSize = Vec3f(5.0f, 1.0f, 10.0f);
	real mass = (real) 1e16;
	Quatf orientation = Quatf( 20.0f / 180.0f * 3.14f , 1.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );

	PhysicsObject *planeBox = new Box;
	planeBox->vSetState( 0.0f, 8.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(planeBox);

	// Single box
	halfSize = Vec3f(.5f, .5f, .5f);
	mass = (real) 1.0f;
	orientation = Quatf(20.0f / 180.0f * 3.14f , 1.0f, 0.0f, 0.0f);
	acceleration = Vec3f(0.0f, -10.0f, 0.0f );
	PhysicsObject *singleBox = new Box;
	singleBox->vSetState( 0.0f, 14.0f, 5.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(singleBox);

	halfSize = Vec3f(10.0f, 1.0f, 30.0f);
	mass = (real) 1e16;
	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	acceleration = Vec3f(0.0f, 0.0f, 0.0f );
	PhysicsObject *bigBox = new Box;
	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);
}

void _billiards( std::vector<TorPhysix::PhysicsObject*> &physObjList )
{
	for(size_t i=0; i<physObjList.size(); i++)
	{
		delete physObjList[i];
	}
	physObjList.clear();

	for(int i=0; i<5; i++)
	{
		Vec3f halfSize = Vec3f(0.5f, 0.5f, 0.5f);
		real mass = (real) 100.0;
		Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
		Vec3f acceleration = Vec3f(0.0f, -10.0f, 0.0f );

		PhysicsObject *ball = new Ball;
		physObjList.push_back(ball);
		real x = (real)i + i * 0.01f;
		real y = (real)0.5f;
		real z = (real)0.0f;
		
		if( i==0 )
			x = -1.0f;

		ball->vSetState( x,y,z, orientation, halfSize, mass, acceleration);
		
		if( i==0 )
		{			
			ball->collisionPrimitive->body->setVelocity( Vec3f(50.0f, 0.0f, 0.0f) );
		}

		
	}

	Vec3f halfSize = Vec3f(10.0f, 1.0f, 30.0f);
	real mass = (real) 1e16;
	Quatf orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	Vec3f acceleration = Vec3f(0.0f, 0.0f, 0.0f );
	PhysicsObject *bigBox = new Box;
	bigBox->vSetState( 0.0f, -1.0f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(bigBox);

	halfSize = Vec3f(.5f, .5f, 3.5f);
	mass = (real) 1e16f;
	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	acceleration = Vec3f(0.0f, 0.0f, 0.0f );
	PhysicsObject *singleBox1 = new Box;
	singleBox1->vSetState( 10.0f, 0.5f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(singleBox1);

	halfSize = Vec3f(.5f, .5f, 3.5f);
	mass = (real) 1e16f;
	orientation = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
	acceleration = Vec3f(0.0f, 0.0f, 0.0f );
	PhysicsObject *singleBox2 = new Box;
	singleBox2->vSetState( -10.0f, 0.5f, 0.0f, orientation, halfSize, mass, acceleration);
	physObjList.push_back(singleBox2);
}

App::App() : GLFWApp()
{
	wireframe = false;
	camZoom = 10;
	camVertical = 25.0f;
	camHorizontal = 0.0f;
	oldMouseX = oldMouseY = 0;
	BitmapFont::Init();
	simIterations = 0;
	mousePointerVelocity.is(0.0f, 0.0f, 0.0f);
	mousePointer.is(0,0,0);
	oldMousePointer.is(0,0,0);

	/************************************************************************/
	/*           Create and setup collision objects, then reset state       */
	/************************************************************************/

	resetPhysics();
	_spheresVsBoxes( physObjList );
	
	glfwSetWindowTitle("Test suite. Press 1, 2, 3");
	mainloop();
}

void App::update()
{
	
	float mouseXSpeed = (float) Input::mouseX() - oldMouseX;
	float mouseYSpeed = (float) Input::mouseY() - oldMouseY;

	oldMouseX = Input::mouseX();
	oldMouseY = Input::mouseY();

	if( Input::MouseDown(GLFW_MOUSE_BUTTON_LEFT) ) 
	{
		camVertical += mouseYSpeed * 0.1f;
		camHorizontal += mouseXSpeed * 0.1f;
	}

	if( Input::MouseDown(GLFW_MOUSE_BUTTON_RIGHT) )
	{
		camZoom += mouseYSpeed * 0.1f;
	}


	stepOneFrame = Input::keyHit(GLFW_KEY_SPACE);
	simulateRealtime = Input::keyDown('S');


	if( stepOneFrame || simulateRealtime ) 
	{
		double phys_start = glfwGetTime();
		real timestep = (real) 0.01;
		//if( stepOneFrame ) timestep =  (real) 0.01;
		//else if( simulateRealtime ) timestep = (real)dt;

		if( timestep <= 0.0f ) return;
		// prevent from exploding due to too high timestep
		if( timestep > 0.01f )
		{
			printf("warn: high timestep\n");
			timestep = 0.01f;
		}

		for (int iters=0; iters < 10; iters++)
		{
			real time2 = (real) (timestep * .1f);
			updateObjects(time2);

			// Generate contacts for resting objects and new collisions
			contacts.resize( physObjList.size() * 8 );
			cData.contactArray = &contacts[0];
			cData.contactCount = contacts.size();
			generateContacts();

			// Resolve detected contacts
			resolver.resolveContacts( cData.contactArray, cData.contactCount, time2 );
		}



		simIterations++;
		physicsTime = glfwGetTime() - phys_start;
	}



}


// \breif Expects OpenGL modelview matrix to be set to world view. for instance with gluLookAt
void App::updateWorldMouseCoordinates()
{
	// TODO see if i remember how i did this. get near pixel perfect pick plane infront of camera view. 
	// is the same as a section of view frustum
	float xresf = xres / 100.0f;
	float yresf = yres / 100.0f;

	float ratio_x = xresf * camZoom * 1.0f / 15.0f;
	float ratio_y = yresf * camZoom * 1.0f / 15.0f;
	
	Vec2f mousePos( Input::mouseX()/(xres/1.0f) * ratio_x - ratio_x/2.0f, -Input::mouseY()/(yres/1.0f) * ratio_y + ratio_y/2.0f );
	Vec3f centerOfInterest( 0.0f, 0.0f, 0.0f );


	float mtx[16];
	glGetFloatv( GL_MODELVIEW_MATRIX, mtx );

	Mat4f mvp( mtx );
	Mat4f mvp_Inv;
	mvp_Inv = inverse(mvp);

	
	mousePointer = mousePosOnInterestPlaneAsObjectCoords(mvp, mvp_Inv, centerOfInterest, mousePos);
	Vec3f mousePointerDelta = mousePointer - oldMousePointer;

	if( length(mousePointerDelta) > 0.01f )
	{
		mouseSamples[mouseSampleCount] = mousePointerDelta;
		mouseSampleCount++;
		if(mouseSampleCount >= 5) mouseSampleCount = 0;
	}
	oldMousePointer = mousePointer; // save previous so we can get mouse direction

	// Get average direction
	mousePointerVelocity.is(0,0,0);
	for(int i=0; i<mouseSampleCount; i++){
		mousePointerVelocity += mouseSamples[i];
	}
	mousePointerVelocity *= 1.0f / 5.0f;

}

void App::render()
{
	double renderStart = glfwGetTime();
	static unsigned int timesHit = 0;
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	setDepthProjection();
	glLoadIdentity();

	glEnable(GL_DEPTH_TEST);

	gluLookAt( camZoom, 0.0, 0.0,
		0.0, 0.0, 0.0,
		0.0, 1.0, 0.0 );

	glRotatef(-camVertical, 0, 0, 1);
	glRotatef(camHorizontal, 0, 1, 0);

	// Update mouse world coords after setting view transformation
	updateWorldMouseCoordinates();

	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	glDisable(GL_LIGHTING);
	glDisable(GL_DEPTH_TEST);
	glColor4f(0,0,0,0.1f);
	for(size_t i=0; i<physObjList.size(); i++)
	{	
		physObjList[i]->vRenderShadow(); 
	}

	glEnable(GL_DEPTH_TEST);
	glDisable(GL_BLEND);


	float lightPosition[] = {25.0, 50.0, 25.0, 1.0};
	glUseProgram(0);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glLightfv( GL_LIGHT0, GL_POSITION, lightPosition );
	glEnable(GL_NORMALIZE);
	glEnable(GL_COLOR_MATERIAL);

	glTranslatef( lightPosition[0], lightPosition[1], lightPosition[2] );
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glColor3f(1,1,1);
	Shapes3D::DrawSphere(1.0f, 8, 10);
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glTranslatef( -lightPosition[0], -lightPosition[1], -lightPosition[2] );

	glColor3ub(128,128,128);
	if( wireframe )
	{
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	}
	for(size_t i=0; i<physObjList.size(); i++)
	{	
		physObjList[i]->vRender(); 
	}
	if( wireframe ) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glDisable(GL_NORMALIZE);

	// Draw with no shading
	glDisable(GL_LIGHTING);
	



	// Draw some scale circles
	glColor3f(0.75, 0.75, 0.75);
	int numCircles = 10;
	for (int i = 1; i < numCircles; i++)
	{
		glBegin(GL_LINE_LOOP);
		for (int j = 0; j < 32; j++)
		{
			float theta = 6.28f * j / 32.0f;
			glVertex3f(i*cosf(theta),0.01f,i*sinf(theta));
		}
		glEnd();
	}
	glBegin(GL_LINES);
	glVertex3f(-20,0.01f,0);
	glVertex3f(20,0.01f,0);
	glVertex3f(0,0.01f,-20);
	glVertex3f(0,0.01f,20);
	glEnd();

	renderDebugInfo();

	setOrthoProjection();

	char strBuf[200];

	sprintf_s(strBuf,200, "Press and hold space to advance simulation. Sim iters = %i", simIterations );
	BitmapFont::Print(strBuf,15,15*1,0xffffff);

	if( stepOneFrame )
	{
		sprintf_s(strBuf,200,"sim running", 42 );
		BitmapFont::Print(strBuf,xres-150,15*2,0xffffff);
	}

	renderTime = glfwGetTime() - renderStart;

	sprintf_s(strBuf,200,"FPS = %.3f", fps);
	BitmapFont::Print(strBuf,15,15*2,0xffffff);	
	sprintf_s(strBuf,200,"Num contacts: %3i of max %3i", cData.contactCount,  contacts.size() );
	BitmapFont::Print(strBuf,15,15*3,0xffffff);	
	sprintf_s(strBuf,200,"Num pos iters used: %3i of max %3i", resolver.positionIterationsUsed, 3860 );
	BitmapFont::Print(strBuf,15,15*4,0xffffff);	
	sprintf_s(strBuf,200,"Num vel iters : %3i of max %3i", resolver.velocityIterationsUsed, 3860 );
	BitmapFont::Print(strBuf,15,15*5,0xffffff);	
	
	if ( simulateRealtime || stepOneFrame )
	{
		sprintf_s(strBuf,200,"rendertime %f phystime %f phys %.2f pct of frame, render %.2f pct of frame", renderTime, physicsTime, physicsTime/dt, renderTime/dt );
		BitmapFont::Print(strBuf,15,15*6,0xffffff);	
	}
}

void App::renderDebugInfo()
{
	// Recalculate the contacts, so they are current (in case we're
	// paused, for example).
	generateContacts();

	// Render the contacts, if required
	for (unsigned i = 0; i < cData.contactCount; i++)
	{
		// Inter body contacts are in green, floor contacts are red.
		if (contacts[i].body[1]) {
			glColor3f(0,1,0);
		} else {
			glColor3f(1,0,0);
		}

		glPushMatrix();
		Vec3f correct = contacts[i].contactPoint;// - contacts[i].contactNormal;
		glTranslatef( correct.x(), correct.y(), correct.z() );
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		Shapes3D::DrawSphere(0.05f, 12,12);
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		glPopMatrix();

		Vec3f vec = contacts[i].contactPoint;
		glBegin(GL_LINES);
		glVertex3f(vec.x(), vec.y(), vec.z());

		vec += contacts[i].contactNormal * 0.1f;
		glVertex3f(vec.x(), vec.y(), vec.z());
		glEnd();
	}

	std::vector<TorPhysix::PhysicsObject*>::iterator it;
	for(it = physObjList.begin(); it != physObjList.end(); ++it)
	{
		(*it)->vRenderDebug();
	}
	
}

void App::generateContacts()
{
	// Setup contacts
	cData.reset( contacts.size() );
	cData.friction = (real)0.9f; //.6
	//cData.friction = (real)0.9;
	//cData.restitution = (real)0.6;
	cData.restitution = (real)0.6;
	cData.tolerance = (real) 0.1f;
	//cData.tolerance = (real)0.1;


	for(size_t i=0; i<physObjList.size(); i++)
	{
		for(size_t j=i+1; j<physObjList.size(); j++)
		{
			physObjList[i]->acceptVisitor( *(physObjList[j]), &cData );
		}
	}
	
}

void App::updateObjects( real duration )
{
	physicsWorld.startFrame();
	//physicsWorld.runPhysics(duration);

	for(size_t i=0; i<physObjList.size(); i++)
	{
		physObjList[i]->update(duration);
	}

}


void App::resetPhysics()
{
	// Reset the contacts
	cData.contactCount = 0;

	// Reset simulation step count
	simIterations = 0;

	oldMousePointer = mousePointer;
	mouseSampleCount = 0;
}

void App::onKey( const int key, const bool pressed )
{
	if( pressed )
	{
		switch(key)
		{
		case '1':
			_spheresVsBoxes( physObjList );
			break;
		case '2':
			_dominos( physObjList );
			break;
		case '3':
			_cubeStack( physObjList );
			break;
		case '4':
			_jengaCubeStack( physObjList );
			break;
		case '5':
			_inclinedPlane( physObjList );
			break;
		case '6':
			_billiards( physObjList );	
			break;
		case '7':
			_funnel( physObjList );
			break;
		}
		

		if (key == 'W')
		{
			wireframe = !wireframe;
		}
	}

	// Relay key events to the standard handler so we can use Input::isDown etc.
	GLFWApp::onKey(key, pressed);
}

